/*
 * @Author: your name
 * @Date: 2021-05-01 15:45:59
 * @LastEditTime: 2021-05-06 16:01:50
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\Middlewares\Device\MOTOR.h
 */
#ifndef _MOTOR_H
#define _MOTOR_H

#include "MK60_ftm.h"
#include "pid.h"

#define MAX_ENCODERS_SIZE 5

class motor{

public:
	motor(){}
	
	motor(FTMn ftmn, FTM_CHn _ch_positive, FTM_CHn _ch_negative){
		ftm = ftmn;
		ch_positive = _ch_positive;
		ch_negative = _ch_negative;
		pid.SetPIDParam(0, 0, 0, 0, 0);
	}
	virtual ~motor() {}

	int encoders[MAX_ENCODERS_SIZE];	// 取5个编码器值做滤波
	int encoder;						// 计算后的编码器的值
	float target_speed;					// 目标速度，cm/s
	float current_speed;				// 当前速度，cm/s
	int pwm_out;
	myPID pid;

	void init();						// 对ftm进行初始化，务必执行
	void setTarget(float target);		// 设置目标速度，cm/s
	void updateSpeed(int _encoder);		// 读取编码器的值并进行滤波处理，更新当前速度
	void control();						// pid解算以及pwm输出
	void setPWM(int pwm);
	
private:
	const float encode_to_speed = 4.0f;
	const int pwm_frequency = 17000;
	FTMn ftm;
	FTM_CHn ch_positive;
	FTM_CHn ch_negative;
	
	void handleEncoder();	
};

#endif
